#include "../includes.h"


/* Kalman Filter */
void init_matrics(Matrics *, int, int, int);
Matrics multiply(Matrics ,Matrics);
Matrics multiplyN(Matrics ,double);
Matrics Madd(Matrics ,Matrics);
Matrics transpose(Matrics);
double   MatToFloat(Matrics);


/* ---------------------------------------------------------------------
Last Updated  : 98-03-05 2:18
Function Name : 
Description   : 
Parameters    :
Return Values :
--------------------------------------------------------------------- */
void init_matrics(Matrics *A, int row, int col, int data)
{
	int i,j;

	A->Row = row;	A->Col = col;

	for( i = 0; i < col; i++){
		for(j = 0; j < row; j++){
			A->mat[i][j] = data;
		}
	}
}

Matrics multiply(Matrics A,Matrics B)
{
	int i,j,k;
	double sum;

	init_matrics(&R,A.Row,B.Col,0);

	for(i = 0; i < A.Row; i++){
		for(j = 0; j < B.Col; j++){
			for(k = 0,sum = 0; k < A.Col; k++){
				sum += A.mat[i][k]*B.mat[k][j];
			}
			R.mat[i][j] = sum;
		}
	}
	return R;
}



Matrics multiplyN(Matrics A,double n)
{
	int i,j;

	init_matrics(&R,A.Row,A.Col,0);


	for(i = 0; i < A.Row; i++){
		for(j = 0; j < A.Col; j++){
			R.mat[i][j] = A.mat[i][j]*n;
		}
	}
	return R;
}


Matrics Madd(Matrics A,Matrics B)
{
	int i,j;

	init_matrics(&R,A.Row,B.Col,0);


	for(i = 0; i < A.Row; i++){
		for(j = 0; j < B.Col; j++){
			R.mat[i][j] = A.mat[i][j] + B.mat[i][j];
		}
	}
	return R;
}


double MatToFloat(Matrics A)
{
	return A.mat[0][0];
}


Matrics transpose(Matrics A)
{
	int i,j;

	init_matrics(&R,A.Col,A.Row,0);
	for(i = 0; i < R.Row; i++){
		for(j = 0; j < R.Col; j++){
			R.mat[i][j] = A.mat[j][i];
		}
	}
	return R;
}


